#ifndef ALGORITHM_H
#define ALGORITHM_H

#include "../basic/basic.h"

#ifdef __cplusplus
extern "C"
{
    class pidController
    {
    private:
        float Kp = 0;              // 比例增益
        float Ki = 0;              // 积分增益
        float Kd = 0;              // 微分增益
        float max_output = 0;      // 输出限幅
        float integral = 0;        // 积分累积值
        float prev_error = 0;      // 前一次误差（用于积分和微分）
        float prev_derivative = 0; // 前一次微分值（用于滤波）
        float tau = 0;             // 微分低通滤波时间常数

    public:
        pidController(float Kp, float Ki, float Kd, float max_output, float tau = 0.2)
            : Kp(Kp), Ki(Ki), Kd(Kd), max_output(max_output), tau(tau) {};
        float update(float error, float dt);
    };
}
#endif
#endif // ALGORITHM_H